#include "min_distance.h"

MinDistance::MinDistance(std::string engine_path, float conf_thres, float nms_thresh, ros::NodeHandle n)
: DetectNode(engine_path, conf_thres, nms_thresh), n_(n)
{   
    getParam();

    cloud_.reset(new pcl::PointCloud<PointT>());
    cloud_voxel_.reset(new pcl::PointCloud<PointT>());

    pub_img_ = n_.advertise<sensor_msgs::Image>(topic_img_new_, 10);
    pub_box_ = n_.advertise<detect_msgs::Boxes>(topic_box_, 10);
    pub_cloud_ = n.advertise<sensor_msgs::PointCloud2>(topic_cloud_new_, 10);
    pub_cloud_box_ = n.advertise<sensor_msgs::PointCloud2>(topic_cloud_box_, 10);
    msg_cloud_cam_.reset(new sensor_msgs::PointCloud2);
    msg_cloud_box_.reset(new sensor_msgs::PointCloud2);

        // 5 同步函数定义
    sub_cloud_.subscribe(n_, topic_cloud_, 10);
    sub_img_.subscribe(n_, topic_img_, 10);

    sync_ = std::make_shared<Sync>( MySyncPolicy(10), sub_cloud_, sub_img_ );
    sync_->registerCallback( boost::bind(&MinDistance::callBack, this, _1, _2) ); 
}

void MinDistance::callBack(const sensor_msgs::PointCloud2ConstPtr& msg_cloud, const sensor_msgs::ImageConstPtr& msg_img)
{      
    cv::Mat img_ = cv_bridge::toCvShare(msg_img, "bgr8")->image;
    cv::Mat img_det = img_.clone();
    std::vector<Detection> res;
    auto t1 = std::chrono::high_resolution_clock::now();
    inference(img_det, res);
    if(is_filter_)
    {
        pcl::fromROSMsg(*msg_cloud, *cloud_);  // 雷达坐标系,x为深度
        filterVoxel(cloud_, cloud_voxel_, filter_size_);
    }
    else
    {
        pcl::fromROSMsg(*msg_cloud, *cloud_voxel_);
    }

         // 生成距离图像
    pcl::PointCloud<PointT>::Ptr cloud_cam(new pcl::PointCloud<PointT>);
    
    pcl::transformPointCloud(*cloud_voxel_, *cloud_cam, Eigen::Affine3d(Tcl_)); // 点云转到相机坐标下
    // pcl::transformPointCloud(*cloud_cam, *cloud_uv, Eigen::Affine3d(K_));   // 像素坐标但未归一化
    Eigen::Vector3d p_uv(0,0,1);
    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_uvZ(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointXYZRGB p;

    for(auto point : cloud_cam->points)
    {   
    
        if(point.z < 0 || point.z > 50)   // 只处理激光坐标系下x正方向的值, 向前
            continue;

        // 像素坐标归一化
        p_uv[0] = point.x * fx_ / point.z + cx_;
        p_uv[1] = point.y * fy_ / point.z + cy_;

        if(p_uv[0] >= 0 && p_uv[0] <= img_.cols &&
        p_uv[1] >= 0 && p_uv[1] <= img_.rows)
        {   
            p.x = p_uv[0];
            p.y = p_uv[1];
            p.z = point.z;
            p.b = img_.ptr<uchar>(int(p_uv[1]))[int(p_uv[0]) * 3];
            p.g = img_.ptr<uchar>(int(p_uv[1]))[int(p_uv[0]) * 3 + 1];
            p.r = img_.ptr<uchar>(int(p_uv[1]))[int(p_uv[0]) * 3 + 2];
            
            cloud_uvZ->points.emplace_back(p); 
            
            p.x = point.x;
            p.y = point.y;
            cloud_rgb->points.emplace_back(p);
        }
    }   
     
     std::cout << "cloud_cam->points.size():" << cloud_cam->points.size() << std::endl;
     std::cout << "cloud_uvZ->points.size():" << cloud_uvZ->points.size() << std::endl;
     std::cout << "cloud_rgb->points.size():" << cloud_rgb->points.size() << std::endl;

    
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb_box(new pcl::PointCloud<pcl::PointXYZRGB>);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZRGB>); // 中间件
    

    msg_boxes_.Boxes.clear();

    if(res.size())
    {
        for(auto it : res)
        {
            cv::Rect box = get_rect(img_, it.bbox);  // 恢复尺度
            cloud1->points.clear();
            pcl::copyPointCloud(*cloud_uvZ, *cloud1);

            passThroughFilter(cloud1, box);

            if(!cloud1->points.size())
                continue;
            Eigen::Vector3d cloud_min(0, 0, 50);
            // 处理分割出来的点云cloud2
            for (auto point : cloud1->points)
            {
                if(point.z < cloud_min[2])
                    cloud_min = Eigen::Vector3d(point.x, point.y, point.z);
                
                point.x = (point.x - cx_)  * point.z  / fx_;  // 像素还原到相机
                point.y = (point.y - cy_)  * point.z  / fy_;
                cloud_rgb_box->points.emplace_back(point);
            }

            float x =  (cloud_min[0] - cx_)  * cloud_min[2] / fx_;
            float y =  (cloud_min[1] - cy_)  * cloud_min[2] / fy_;

            msg_box_.class_name = class_names[it.class_id];
            msg_box_.id = it.class_id;
            msg_box_.score = it.conf;
            msg_box_.x = box.x;
            msg_box_.y = box.y;
            msg_box_.w = box.width;
            msg_box_.h = box.height;
            msg_box_.p_z = cloud_min[2];
            msg_box_.p_x = x;
            msg_box_.p_y = y; 

            msg_boxes_.Boxes.emplace_back(msg_box_);

            std::cout << msg_box_.class_name << " " 
                << msg_box_.x << " " << msg_box_.y << " " << msg_box_.w << " " << msg_box_.h << " "
                << msg_box_.p_x << " " << msg_box_.p_y << " " << msg_box_.p_z << " "
                << msg_box_.score << std::endl;

            drawBoxAndDist(img_det, msg_box_);

        }
    }

   
    auto t2 = std::chrono::high_resolution_clock::now();
    auto cost_time = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);

    msg_img_new_ = cv_bridge::CvImage(msg_cloud->header, "bgr8", img_det).toImageMsg();
    pub_img_.publish(msg_img_new_);

    pub_box_.publish(msg_boxes_);  // 发布检测结果
    
    pcl::toROSMsg(*cloud_rgb_box, *msg_cloud_box_);
    msg_cloud_box_->header.stamp = msg_cloud->header.stamp;
    msg_cloud_box_->header.frame_id = frame_id_;
    pub_cloud_box_.publish(msg_cloud_box_);

    pcl::toROSMsg(*cloud_rgb, *msg_cloud_cam_);
    msg_cloud_cam_->header.stamp = msg_cloud->header.stamp;
    msg_cloud_cam_->header.frame_id = frame_id_;
    pub_cloud_.publish(msg_cloud_cam_);
    
    ROS_INFO("Detection and ranging is %.2f fps ...", 1000.0 / cost_time.count());

}

void MinDistance::filterVoxel(const pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<PointT>::Ptr& cloud_out, float size)
{
    pcl::VoxelGrid<PointT> sor; // 创建体素滤波器
    sor.setInputCloud(cloud_in);          // 输入点云
    // sor.setLeafSize(0.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体,m
    sor.setLeafSize(size, size, size);
    sor.filter(*cloud_out);
}

void MinDistance::Rt2T(Eigen::Isometry3d& T)
{
    // R,t->Tcl 
    Eigen::Vector3d R_euler(roll_, pitch_, yaw_); // 旋转
    R_euler = R_euler / 180 * M_PI;
    
    Eigen::Vector3d t(x_, y_, z_);  // 平移
    Eigen::Matrix3d R;
    R = Eigen::AngleAxisd(R_euler[0], Eigen::Vector3d::UnitZ()) * 
        Eigen::AngleAxisd(R_euler[1], Eigen::Vector3d::UnitY()) * 
        Eigen::AngleAxisd(R_euler[2], Eigen::Vector3d::UnitX());
    T.rotate(R);
    T.pretranslate (t);
}

void MinDistance::drawBoxAndDist(cv::Mat& img_input , const detect_msgs::Box& it)
{
    // std::cout << detector_->class_names_[it.class_idx] << " " << it.score << " " << dist << std::endl;

    cv::Rect box= cv::Rect(it.x, it.y, it.w, it.h);
    // cv::Rect box = get_rect(img_input, it.bbox); 

    cv::rectangle(img_input, box, cv::Scalar(0, 0, 255), 2);  // 画矩形
    
    std::stringstream ss;
    ss << std::fixed << std::setprecision(2) << it.score;
    std::string s =class_names[it.id] + " " + ss.str();
    
    auto font_face = cv::FONT_HERSHEY_DUPLEX;
    auto font_scale = 0.8;
    int thickness = 1;
    int baseline=0;
    auto s_size = cv::getTextSize(s, font_face, font_scale, thickness, &baseline);
    
    cv::rectangle(img_input,
        cv::Point(box.x, box.y - s_size.height - 5),
        cv::Point(box.x + s_size.width, box.y),
        cv::Scalar(0, 0, 255), -1);
    
    cv::putText(img_input, s, cv::Point(box.x, box.y - 5),
            font_face , font_scale, cv::Scalar(255, 255, 255), thickness);

    std::stringstream ss2;
    
    double z = sqrt( it.p_x * it.p_x + it.p_y * it.p_y + it.p_z * it.p_z );
    ss2 << std::fixed << std::setprecision(2) << z;  // 激光坐标是x表示深度
    
    std::cout << s << " dist: " << ss2.str() << std::endl;
    
    s = "Dist: " + ss2.str();
    cv::circle(img_input, cv::Point(box.x + box.width / 2,  box.y + box.height / 2),
                 3, cv::Scalar(0, 0, 255), -1 );
    cv::putText(img_input, s, cv::Point(box.x + box.width / 2, box.y + box.height / 2),
            font_face , font_scale, cv::Scalar(0, 0, 255), thickness);    
}

void MinDistance::passThroughFilter( pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_in, const cv::Rect& box)
{
    pcl::PointCloud<pcl::PointXYZRGB> cloud_x, cloud_y;
    pcl::PassThrough<pcl::PointXYZRGB> pass_x;  // 创建直通滤波器对象
    pass_x.setInputCloud(cloud_in);            // 设置输入对象,传地址
    pass_x.setFilterFieldName("x");             // 设置过滤时所需要点云类型的X字段
    pass_x.setFilterLimits(box.x, box.x + box.width);   
    pass_x.filter(*cloud_in);     
    
    pcl::PassThrough<pcl::PointXYZRGB> pass_y; 
    pass_y.setInputCloud(cloud_in);  
    pass_y.setFilterFieldName("y");             // 设置过滤时所需要点云类型的Y字段
    pass_y.setFilterLimits(box.y, box.y + box.height );   
    pass_y.filter(*cloud_in);

}



void MinDistance::getParam()
{
    // detector_->weight_path_ = detector_->pkg_path_ + "/weights/n_cpu.torchscript";
    n_.getParam("filter_size", filter_size_);
    n_.param<float>("filter_size", filter_size_, 0.2);
    
    n_.getParam("is_filter", is_filter_);
    n_.param<bool>("is_filter", is_filter_, false);

    // n_.getParam("find_point_radius", find_point_radius_);
    // n_.param<float>("find_point_radius", find_point_radius_, 40);

    n_.getParam("frame_id", frame_id_);
    n_.param<std::string>("frame_id", frame_id_, "map"); // 左右二维码中心距
    
    n_.getParam("topic_img", topic_img_);
	n_.param<std::string>("topic_img", topic_img_, "/image_left");

    n_.getParam("topic_cloud", topic_cloud_);
    n_.param<std::string>("topic_cloud", topic_cloud_, "/velodyne_points");
    
	n_.getParam("topic_img_new", topic_img_new_);
	n_.param<std::string>("topic_img_new", topic_img_new_, "/detect/image");
    
    n_.getParam("topic_box", topic_box_);
    n_.param<std::string>("topic_box", topic_box_, "/detect/boxes");

    n_.getParam("topic_cloud_new", topic_cloud_new_);
    n_.param<std::string>("topic_cloud_new", topic_cloud_new_, "/detect/cloud_cam");

    n_.getParam("topic_cloud_box", topic_cloud_box_);
    n_.param<std::string>("topic_cloud_box", topic_cloud_box_, "/detect/cloud_boxes");
    


    // -------------内参-----------------------
    n_.getParam("fx", fx_);
    n_.param<float>("fx", fx_, 718.856); 
    n_.getParam("fy", fy_);
    n_.param<float>("fy", fy_, 718.856); 
    n_.getParam("cx", cx_);
    n_.param<float>("cx", cx_, 607.1928); 
    n_.getParam("cy", cy_);
    n_.param<float>("cy", cy_, 185.2157); 
    
    n_.getParam("k1", k1_);
    n_.param<float>("k1", k1_, 0.0); 
    n_.getParam("k2", k2_);
    n_.param<float>("k2", k2_, 0.0); 
    n_.getParam("k3", k3_);
    n_.param<float>("k3", k3_, 0.0); 
    
    n_.getParam("p1", p1_);
    n_.param<float>("p1", p1_, 0.0); 
    n_.getParam("p2", p2_);
    n_.param<float>("p2", p2_, 0.0); 

    //  --------Tcl--------
    n_.getParam("roll", roll_);
    n_.param<float>("roll", roll_, 90.); 
    n_.getParam("pitch_", pitch_);
    n_.param<float>("pitch", pitch_, -90.); 
    n_.getParam("yaw", yaw_);
    n_.param<float>("yaw", yaw_, 0.0); 
    
    n_.getParam("x", x_);
    n_.getParam("y", y_);
    n_.getParam("z", z_);

    n_.param<float>("x", x_, -1.198459927713e-02);  
    n_.param<float>("y", y_, 5.403984729748e-02); 
    n_.param<float>("z", z_, -2.921968648686e-01); 

    // n_.param<float>("x", x_, 0); 
    // n_.param<float>("y", y_, 0);
    // n_.param<float>("z", z_, 0); 

    cameraMatrix_ = (cv::Mat_<float>(3, 3) << fx_, 0., cx_,
                                            0, fy_, cy_,
                                            0, 0., 1);
    
    distCoeffs_ = (cv::Mat_<float>(5, 1) << k1_, k2_, p1_, p2_, k3_);  // 初始化加（）
    
    Eigen::Matrix3d K;
    cv::cv2eigen(cameraMatrix_, K);
    K_.rotate(K);
    K_.pretranslate(Eigen::Vector3d (0, 0, 0));

    Rt2T(Tcl_);

    std::cout << "\n---------------\n" << std::endl;
    std::cout << " --cameraMatrix: \n" << K_.matrix() << std::endl;
    std::cout << "\n --distCoeffs: \n"<< distCoeffs_.at<float>(0,0) << " " <<
                                    distCoeffs_.at<float>(1,0) << " " <<
                                    distCoeffs_.at<float>(2,0) << " " <<
                                    distCoeffs_.at<float>(3,0) << " " <<
                                    distCoeffs_.at<float>(4,0) << " " << std::endl;
    std::cout << "\n --T_lidar2cam:\n" << Tcl_.matrix() << std::endl;
    
    std::cout << "\n-------------------\n" << std::endl;
    
    std::cout << " --filter_size : "<< filter_size_ << std::endl;
    std::cout << " --is_filter: " << is_filter_ << std::endl; 

    std::cout << " --topic_img: " << topic_img_ << std::endl; 
    std::cout << " --topic_cloud: " << topic_cloud_ << std::endl; 
    
    std::cout << " --frame_id: " << frame_id_ << std::endl; 
    std::cout << " --topic_img_new: " << topic_img_new_ << std::endl; 
    std::cout << " --topic_box: " << topic_box_ << std::endl; 
    std::cout << " --topic_cloud_new : " << topic_cloud_new_ << std::endl; 
     std::cout << " --topic_cloud_box : "<< topic_cloud_box_ << std::endl;

    std::cout << "\n---------------\n" << std::endl;
}
